#include "stm32f10x.h"                  // Device header
#include "OLED.h"
#include "Encoder.h"
#include "Delay.h"

#define BIN1_1(x)		GPIO_WriteBit(GPIOA, GPIO_Pin_4, (BitAction)(x))
#define BIN2_1(x)		GPIO_WriteBit(GPIOA, GPIO_Pin_5, (BitAction)(x))

#define AIN1_1(x)		GPIO_WriteBit(GPIOA, GPIO_Pin_8, (BitAction)(x))
#define AIN2_1(x)		GPIO_WriteBit(GPIOE, GPIO_Pin_13, (BitAction)(x))


#define BIN1_2(x)		GPIO_WriteBit(GPIOE, GPIO_Pin_7, (BitAction)(x))
#define BIN2_2(x)		GPIO_WriteBit(GPIOE, GPIO_Pin_8, (BitAction)(x))

#define AIN1_2(x)		GPIO_WriteBit(GPIOG, GPIO_Pin_0, (BitAction)(x))
#define AIN2_2(x)		GPIO_WriteBit(GPIOG, GPIO_Pin_1, (BitAction)(x))

int16_t TIM3_maichong,TIM5_maichong,TIM1_maichong,TIM4_maichong;
int16_t PWM_TIM5,PWM_TIM3,PWM_TIM1,PWM_TIM4;

float left_targetspeed;
float right_targetspeed;

float leftForward_targetspeed;//左前轮目标速度
float rightForward_targetspeed;//右前轮目标速度
float leftBackward_targetspeed;//左后轮目标速度
float rightBackward_targetspeed;//右后轮目标速度

float leftForward_targetmaichong;
float rightForward_targetmaichong;
float leftBackward_targetmaichong;
float rightBackward_targetmaichong;

float left_targetmaichong;//之前代码
float right_targetmaichong;//之前代码
float targetdistance;//目标距离
float targetAngle;

float KP=6;
float KI=1;
float KD=0;

int32_t TIM3_leijimaichong;
int32_t TIM5_leijimaichong;
int32_t TIM1_leijimaichong;
int32_t TIM4_leijimaichong;
uint16_t setdistance_states;//执行运动指定距离标志,置1执行,执行完自动恢复0
uint16_t setAngle_states;//执行运动指定角度标志,置1执行,执行完自动恢复0


void car_init(void)
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOE|RCC_APB2Periph_GPIOG, ENABLE);
	GPIO_InitTypeDef GPIO_Structure;
	GPIO_Structure.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_Structure.GPIO_Pin=GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_8;
	GPIO_Structure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_Structure);
	
	GPIO_Structure.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_Structure.GPIO_Pin=GPIO_Pin_13 | GPIO_Pin_7 | GPIO_Pin_8;
	GPIO_Structure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOE,&GPIO_Structure);
	
	GPIO_Structure.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_Structure.GPIO_Pin=GPIO_Pin_0 | GPIO_Pin_1;
	GPIO_Structure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOG,&GPIO_Structure);
}

//TIM8_CH1（左前轮）、TIM8_CH2（左后轮）、TIM8_CH3(右后轮)和TIM8_CH4（右前轮）,PWM输出,分别接PC6、PC7、PC8和PC9
void PWM_car_Init(void)
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9|GPIO_Pin_6|GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOC, &GPIO_InitStructure);
	
	TIM_InternalClockConfig(TIM8);
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_Period = 7200 - 1;		//ARR
	TIM_TimeBaseInitStructure.TIM_Prescaler = 1 - 1;		//PSC
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(TIM8, &TIM_TimeBaseInitStructure);
	
	TIM_OCInitTypeDef TIM_OCInitStructure;
	TIM_OCStructInit(&TIM_OCInitStructure);
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;		//CCR
	TIM_OC3Init(TIM8, &TIM_OCInitStructure);
	TIM_OC1Init(TIM8,&TIM_OCInitStructure);
	TIM_OC2Init(TIM8,&TIM_OCInitStructure);
	TIM_OC4Init(TIM8, &TIM_OCInitStructure);
	
	TIM_CtrlPWMOutputs(TIM8,ENABLE);
	TIM_Cmd(TIM8, ENABLE);
}

//左轮向前
void car_left_forward(void)
{
	BIN1_1(1);//左前
	BIN2_1(0);
	AIN1_1(1);//左后
	AIN2_1(0);
}
//左轮向后
void car_left_backward(void)
{
	BIN1_1(0);//左前
	BIN2_1(1);
	AIN1_1(0);//左后
	AIN2_1(1);
}
//左轮停止
void car_left_stop(void)
{
	BIN1_1(1);//左前
	BIN2_1(1);
	AIN1_1(1);//左后
	AIN2_1(1);
}
//右轮向前
void car_right_forward(void)
{
	BIN1_2(1);//右后
	BIN2_2(0);
	AIN1_2(1);//右前
	AIN2_2(0);
}
//右轮向后
void car_right_backward(void)
{
	BIN1_2(0);//右后
	BIN2_2(1);
	AIN1_2(0);//右前
	AIN2_2(1);
}
//右轮停止
void car_right_stop(void)
{
	BIN1_2(1);//右后
	BIN2_2(1);
	AIN1_2(1);//右前
	AIN2_2(1);
}
//前进
void car_forward()
{
	car_left_forward();
	car_right_forward();
}
//后退
void car_backward(void)
{
	car_left_backward();
	car_right_backward();
}
//左转
void car_left(void)
{
	car_left_backward();
	car_right_forward();
}
//右转
void car_right(void)
{
	car_left_forward();
	car_right_backward();
}
void car_stop(void)
{
	car_left_stop();
	car_right_stop();
}
//SET-TIM8_CH1,PWM,PC6------》左前轮的pwm
void PWM_SetCompare1(uint16_t Compare)
{
	TIM_SetCompare1(TIM8, Compare);
}
//SET-TIM8_CH2,PWM,PC7------》左后轮的pwm
void PWM_SetCompare2(uint16_t Compare)
{
	TIM_SetCompare2(TIM8, Compare);
}

//全部的总PWM设置
void PWM_SUM_SET(uint16_t leftCompare1,uint16_t leftCompare2,uint16_t rightCompare1,uint16_t rightCompare2)
{
	TIM_SetCompare1(TIM8, leftCompare1);
	TIM_SetCompare2(TIM8, leftCompare2);
	TIM_SetCompare3(TIM8, rightCompare1);
	TIM_SetCompare4(TIM8, rightCompare2);
}

//set-TIM8_CH3,PWM,PC8------》右后轮的pwm
void PWM_SetCompare3(uint16_t Compare)
{
	TIM_SetCompare3(TIM8, Compare);
}
//set-TIM8_CH4,PWM,PC9------》右前轮的pwm
void PWM_SetCompare4(uint16_t Compare)
{
	TIM_SetCompare4(TIM8, Compare);
}

//1、左前轮子pid（单个轮子）（PE9和PE11对应的编码器是TIM1，）
void set_left_forward_speed(float targetspeed)
{
	leftForward_targetmaichong = (int16_t)(targetspeed*maichong*Time/C);	//将速度值转换为对应的脉冲数
	int16_t error,d_error,dd_error;
	static int16_t last_error=0,last_last_error=0;
	error = leftForward_targetmaichong -TIM1_maichong;			//e(k)
	d_error =(error-last_error);					//e(k)-e(k-1)
	dd_error=-2*last_error+error+last_last_error;	//[e(k)-2e(k-1)+e(k-2)]
	if(targetspeed>0)		//前进
	{
		BIN1_1(1);//左前
		BIN2_1(0);
		PWM_TIM1 = PWM_TIM1 +(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	else if(targetspeed==0)		//停止
	{
		BIN1_1(1);//左前
		BIN2_1(1);
		PWM_TIM1=0;
	}
	else					//后退
	{
		BIN1_1(0);//左前
		BIN2_1(1);
		PWM_TIM1 = PWM_TIM1 -(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	if(PWM_TIM1<0)PWM_TIM1=0;
	if(PWM_TIM1>7000)PWM_TIM1=7000;
	PWM_SetCompare1(PWM_TIM1);//左前
	//OLED_ShowNum(1, 7,PWM_TIM5, 4);
}
//2、右前轮子pid（单个轮子）（PB6和PB7对应的编码器是TIM4，对应的pwm定时器是PWM_TIM4）
void set_right_forward_speed(float targetspeed)
{
	rightForward_targetmaichong = (int16_t)(targetspeed*maichong*Time/C);	//将速度值转换为对应的脉冲数
	int16_t error,d_error,dd_error;
	static int16_t last_error=0,last_last_error=0;
	error = rightForward_targetmaichong -TIM4_maichong;			//e(k)
	d_error =(error-last_error);					//e(k)-e(k-1)
	dd_error=-2*last_error+error+last_last_error;	//[e(k)-2e(k-1)+e(k-2)]
	if(targetspeed>0)		//前进
	{
		AIN1_2(1);//右前
		AIN2_2(0);
		PWM_TIM4 = PWM_TIM4 +(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	else if(targetspeed==0)		//停止
	{
		AIN1_2(1);//右前
		AIN2_2(1);
		PWM_TIM4=0;
	}
	else					//后退
	{
		AIN1_2(0);//右前
		AIN2_2(1);
		PWM_TIM4 = PWM_TIM4 -(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	if(PWM_TIM4<0)PWM_TIM4=0;
	if(PWM_TIM4>7000)PWM_TIM4=7000;
	PWM_SetCompare4(PWM_TIM4);//右前
	//OLED_ShowNum(1, 7,PWM_TIM5, 4);
}

//3、左后轮子pid（单个轮子）（PA6和PA7对应的编码器是TIM3，）
void set_left_backward_speed(float targetspeed)
{
	leftBackward_targetmaichong = (int16_t)(targetspeed*maichong*Time/C);	//将速度值转换为对应的脉冲数
	int16_t error,d_error,dd_error;
	static int16_t last_error=0,last_last_error=0;
	error = leftBackward_targetmaichong -TIM3_maichong;			//e(k)
	d_error =(error-last_error);					//e(k)-e(k-1)
	dd_error=-2*last_error+error+last_last_error;	//[e(k)-2e(k-1)+e(k-2)]
	if(targetspeed>0)		//前进
	{
		AIN1_1(1);//左后
		AIN2_1(0);
		PWM_TIM3 = PWM_TIM3 +(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	else if(targetspeed==0)		//停止
	{
		AIN1_1(1);//左后
		AIN2_1(1);
		PWM_TIM3=0;
	}
	else					//后退
	{
		AIN1_1(0);//左后
		AIN2_1(1);
		PWM_TIM3 = PWM_TIM3 -(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	if(PWM_TIM3<0)PWM_TIM3=0;
	if(PWM_TIM3>7000)PWM_TIM3=7000;
	PWM_SetCompare2(PWM_TIM3);//右前
	//OLED_ShowNum(1, 7,PWM_TIM5, 4);
}

//4、右后轮子pid（单个轮子）（PA1和PA0对应的编码器是TIM5，）
void set_right_backward_speed(float targetspeed)
{
	rightBackward_targetmaichong = (int16_t)(targetspeed*maichong*Time/C);	//将速度值转换为对应的脉冲数
	int16_t error,d_error,dd_error;
	static int16_t last_error=0,last_last_error=0;
	error = rightBackward_targetmaichong -TIM5_maichong;			//e(k)
	d_error =(error-last_error);					//e(k)-e(k-1)
	dd_error=-2*last_error+error+last_last_error;	//[e(k)-2e(k-1)+e(k-2)]
	if(targetspeed>0)		//前进
	{
		BIN1_2(1);//右后
		BIN2_2(0);
		PWM_TIM5 = PWM_TIM5 +(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	else if(targetspeed==0)		//停止
	{
		BIN1_2(1);//右后
		BIN2_2(1);
		PWM_TIM5=0;
	}
	else					//后退
	{
		BIN1_2(0);//右后
		BIN2_2(1);
		PWM_TIM5 = PWM_TIM5 -(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	if(PWM_TIM5<0)PWM_TIM5=0;
	if(PWM_TIM5>7000)PWM_TIM5=7000;
	PWM_SetCompare3(PWM_TIM5);//右后
	//OLED_ShowNum(1, 7,PWM_TIM5, 4);
}

//左前和左后轮子合并
void set_left2_speed(float left_targetspeed)
{
	set_left_forward_speed(left_targetspeed);
	set_left_backward_speed(left_targetspeed);
}
//右前和右后轮子合并
void set_right2_speed(float right_targetspeed)
{
	set_right_forward_speed(right_targetspeed);
	set_right_backward_speed(right_targetspeed);
}

/*
函数说明：控制小车前后运动的速度
参数：左前后轮和右前后轮的目标速度（m/s）,正数是前进,负数是后退
返回值：无
说明：无
注意：要放在主循环或着定时中断中（即要能定时执行）
*/
void set_speed2(float left_targetspeed,float right_targetspeed)
{
	set_left2_speed(left_targetspeed);
	set_right2_speed(right_targetspeed);
}










//以下代码是之前留下来的代码：：：：：：：：：：


/*
函数功能：调节右轮电机速度(自己写，未用PID)
参数：目标速度（m/s）,正数是前进，负数是后退
返回值：无
说明：无
注意：主循环中用了set_speed()，就能再用这个
*/
void set_right_speed(float targetspeed)
{
	right_targetmaichong = (int16_t)(targetspeed*maichong*Time/C);	//将速度值转换为对应的脉冲数
	int16_t error,d_error,dd_error;
	static int16_t last_error=0,last_last_error=0;
	error = right_targetmaichong -TIM5_maichong;			//e(k)
	d_error =(error-last_error);					//e(k)-e(k-1)
	dd_error=-2*last_error+error+last_last_error;	//[e(k)-2e(k-1)+e(k-2)]
	if(targetspeed>0)		//前进
	{
		BIN1_2(1);//右后
		BIN2_2(0);
		
		BIN1_1(1);//左前
		BIN2_1(0);
		PWM_TIM5 = PWM_TIM5 +(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	else if(targetspeed==0)		//停止
	{
		BIN1_2(1);//右后
		BIN2_2(1);
		
		BIN1_1(1);//左前
		BIN2_1(1);
		PWM_TIM5=0;
	}
	else					//后退
	{
		BIN1_2(0);//右后
		BIN2_2(1);
		
		BIN1_1(0);//左前
		BIN2_1(1);
		PWM_TIM5 = PWM_TIM5 -(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	if(PWM_TIM5<0)PWM_TIM5=0;
	if(PWM_TIM5>7000)PWM_TIM5=7000;
	PWM_SetCompare3(PWM_TIM5);//右后
	PWM_SetCompare1(PWM_TIM5);//左前
	//OLED_ShowNum(1, 7,PWM_TIM5, 4);
}

/*
函数功能：调节左轮电机速度(自己写，未用PID)
参数：目标速度（m/s）,正数是前进，负数是后退
返回值：无
说明：无
注意：主循环中用了set_speed()，就能再用这个
*/
void set_left_speed(float targetspeed)
{
	left_targetmaichong = (int16_t)(targetspeed*maichong*Time/C);	//将速度值转换为对应的脉冲数
	int16_t error,d_error,dd_error;
	static int16_t last_error=0,last_last_error=0;
	error = left_targetmaichong -TIM3_maichong;			//e(k)
	d_error =(error-last_error);					//e(k)-e(k-1)
	dd_error=-2*last_error+error+last_last_error;	//[e(k)-2e(k-1)+e(k-2)]
	if(targetspeed>0)		//前进
	{
		AIN1_2(1);//右前
		AIN2_2(0);
		
		AIN1_1(1);//左后
		AIN2_1(0);
		PWM_TIM3 = PWM_TIM3 +(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	else if(targetspeed==0)		//停止
	{
		AIN1_2(1);//右前
		AIN2_2(1);
		
		AIN1_1(1);//左后
		AIN2_1(1);
		PWM_TIM3=0;
	}
	else					//后退
	{
		AIN1_2(0);//右前
		AIN2_2(1);
		
		AIN1_1(0);//左后
		AIN2_1(1);
		PWM_TIM3 = PWM_TIM3 -(KP*d_error+KI*error+KD*dd_error);	//u(k)
	}
	if(PWM_TIM3<0)PWM_TIM3=0;
	if(PWM_TIM3>7000)PWM_TIM3=7000;
	PWM_SetCompare4(PWM_TIM3+490);//右前
	PWM_SetCompare2(PWM_TIM3);//左后
	//OLED_ShowNum(1, 7,PWM_TIM3, 4);
}




/*
函数说明：控制小车前后运动的速度
参数：左轮和右轮的目标速度（m/s）,正数是前进,负数是后退
返回值：无
说明：无
注意：要放在主循环或着定时中断中（即要能定时执行）
*/
void set_speed(float left_targetspeed,float right_targetspeed)
{
	set_left_speed(left_targetspeed);
	set_right_speed(right_targetspeed);
}




/*
函数说明：控制小车前后移动的距离
参数：目标距离（范围：-621468~621468m（大概））,正数是前进指定距离,负数是后退指定距离
返回值：无
说明：无
注意：要放在主循环或着定时中断中（即要能定时执行）
		传入参数后还要修改setdistance_states的值为1
*/
void set_distance(float targetdistance1)
{
//	float P= 0.00005;
	if(setdistance_states==1)
	{
		TIM5_leijimaichong=0;		//右轮累计脉冲清零
		TIM3_leijimaichong=0;		//左轮累计脉冲清零
		setdistance_states=2;				//脉冲清零标志位清零，表示脉冲不清零，确保累计脉冲在一个移动过程中只清零一次
	}
	if(setdistance_states==2)
	{
		int32_t targetmaichong = (int32_t)(targetdistance1*maichong/C);		//将距离转换为对应的脉冲数（目标脉冲数）
//		targetspeed = (targetmaichong-TIM5_leijimaichong)*P;
		if(targetmaichong>0)//有目标距离要走
		{
			if(targetmaichong-TIM5_leijimaichong<=4146)
			{
				left_targetspeed=0.382;//离目标1.2米之内以0.382m/s的速度运动
				right_targetspeed=0.382;
			}
			else if(targetmaichong-TIM5_leijimaichong<=6910)
			{
				left_targetspeed=0.8;//离目标1.2~2米以0.8m/s的速度运动
				right_targetspeed=0.8;
			}
			else if(targetmaichong-TIM5_leijimaichong<=10366)
			{
				left_targetspeed=1.1;		//离目标2~3米以1.1m/s的速度运动
				right_targetspeed=1.1;
			}
			else
			{
				left_targetspeed=1.4;	//离目标3米开外以1.4m/s的速度运动
				right_targetspeed=1.4;
			}
		}
		else if(targetmaichong<0)
		{
			if(targetmaichong-TIM5_leijimaichong>=-4146)
			{
				left_targetspeed=-0.382;//离目标1.2米之内以0.382m/s的速度运动
				right_targetspeed=-0.382;
			}
			else if(targetmaichong-TIM5_leijimaichong>=-6910)
			{
				left_targetspeed=-0.8;//离目标1.2~2米以0.8m/s的速度运动
				right_targetspeed=-0.8;
			}
			else if(targetmaichong-TIM5_leijimaichong>=-10366)
			{
				left_targetspeed=-1.1;		//离目标2~3米以1.1m/s的速度运动
				right_targetspeed=-1.1;
			}
			else
			{
				left_targetspeed=-1.4;//离目标3米开外以1.4m/s的速度运动
				right_targetspeed=-1.4;
			}

		}
		else//无目标距离要走
		{
			
		}
		if(targetmaichong-TIM5_leijimaichong<=34&&targetmaichong-TIM5_leijimaichong>=-34)	//离目标距离1cm范围内就停止小车
		{
			if(setdistance_states==2)
			{
				left_targetspeed=-left_targetspeed;//反向速度制动
				right_targetspeed=-right_targetspeed;
				Delay_ms(150);
				left_targetspeed=0;//修改左轮速度参数为0
				right_targetspeed=0;//修改右轮速度参数为0
				targetdistance=0;//修改距离参数为0
				TIM3_leijimaichong=0;
				TIM5_leijimaichong=0;
				setdistance_states=0;//修改clear_states为0确保速度置零只会执行一次
			}
		}
	}
}

/*
函数说明：控制小车左右转动的角度
参数：目标角度,正前方是0度，往左是负数，往右是正数
返回值：无
说明：无
注意：要放在主循环或着定时中断中（即要能定时执行）
		传入参数后还要修改setAngle_states的值为1

*/
void set_Angle(float targetAngle1)
{
	if(setAngle_states==1)
	{
		TIM5_leijimaichong=0;		//右轮累计脉冲清零
		TIM3_leijimaichong=0;		//左轮累计脉冲清零
		setAngle_states=2;				//脉冲清零标志位清零，表示脉冲不清零，确保累计脉冲在一个移动过程中只清零一次
		OLED_ShowChar(4,16,'a');
	}
	if(setAngle_states==2)
	{
		OLED_ShowFNum(2,8,targetAngle1,5,2);
		int16_t left_targetmaichong = (int16_t)((1.885*targetAngle1)/360*maichong/C);		//将角度转换为对应的脉冲数（目标脉冲数）
		OLED_ShowFNum(2,1,targetAngle1,4,1);
//		int16_t right_targetmaichong = -left_targetmaichong;
		if(left_targetmaichong<0)//需要左转
		{
			left_targetspeed=-1.0;
			right_targetspeed=1.0;
		}
		else if(left_targetmaichong>0)//需要右转
		{
			left_targetspeed=1.0;
			right_targetspeed=-1.0;
		}
		else//无目标角度要走
		{
			
		}
		if(left_targetmaichong-TIM3_leijimaichong<=34&&left_targetmaichong-TIM3_leijimaichong>=-34)	//离目标距离1cm范围内就停止小车
		{
			if(setAngle_states==2)
			{
				left_targetspeed=-left_targetspeed;
				right_targetspeed=-right_targetspeed;
				Delay_ms(100);
				left_targetspeed=0;//修改左轮速度参数为0
				right_targetspeed=0;//修改右轮速度参数为0
				targetAngle=0;//修改距离参数为0
				TIM3_leijimaichong=0;
				TIM5_leijimaichong=0;
				setAngle_states=0;//修改clear_states为0确保速度置零只会执行一次
			}
		}
	}
}
/*
函数功能：增量式PID调节电机速度
参数：目标速度
返回值：无
说明：
*/
//void set_speed(float targetspeed)
//{
//	int16_t targetmaichong = (int16_t)(targetspeed*1320*0.02/0.382);	//将速度值转换为对应的脉冲数
////	targetmaichong = 55;
//	float KP=100;
//	float KI=0.8; 
//	float KD=1;
//	
//	int16_t error,dd_error;
//	static int16_t pwmout=0,last_error=0,last_last_error=0;
//	error = targetmaichong -TIM5_maichong;			//e(k)
//	d_error =(error-last_error);					//e(k)-e(k-1)
//	dd_error=-2*last_error+error+last_last_error;	//[e(k)-2e(k-1)+e(k-2)]
//	pwmout += (KP*d_error+KI*error+KD*dd_error);	//u(k)
//	last_last_error=last_error;
//	last_error=error;
//	if(pwmout>7000)pwmout=7000;//pwm限幅
//	if(pwmout<=0)pwmout=0;
////	pwmout = 2000;
//	PWM_SetCompare3(pwmout);
//	OLED_ShowNum(1, 7,pwmout, 4);
//}
